package org.scalpel.locodo;

import ioio.lib.api.exception.ConnectionLostException;
import android.util.Log;

/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * LocomotionSingle : Compute motor speed on a single motor according
 * to a dynamic target.
 * It need to be configure with a class instance inherited from MotorInterface.
 * Geometric parameter of wheel are needed to convert encoder value to 
 * meters and radian.
 * International Metric System is systematically used. 
 **/
public class LocomotionSingle extends OdometrySingle {

  public LocomotionSingle(double encoderWheelPerimeter,
      int encoderTickPerRotation) {
    super(encoderWheelPerimeter, encoderTickPerRotation);
    // Create the odometric engine. It needs encoder wheel perimeter, 
    // and the number of tick per rotation of the encoder wheel
    
    fPid.P = 1.;
    fPid.I = 0.;
    fPid.D = 0.;
    fPid.iLimit = 0.;
    
    // initialization
    fMotor = null;
    
    fMaxSpeed = 1.0;
    
    fOldDelta = 0.;
    
    // default robot precision
    fPrecision = 0.01f;
    
    // set current position to 0.
    fPosition = 0.;

    // initial target Reached
    fTarget = fPosition;
    fTargetReached = true;
    
    
    // default values of acceleration limit
    fAccLimit = 0.015; //meter/cycle 
    
    // stop signal initialization
    fRegularFreeze = false;
    fEmergencyFreeze = false;
    
    // ramp enable initialization
    fRampEnable = true;
    
    fLastCurrEncoderValue = 0;
    
  };
  

  /**
   * set right and left motor instance
   * @param motorLeft ODOMotorInterface's pointer of the left motor 
   * @param motorRight ODOMotorInterface's pointer of the right motor 
   */
  public void setMotor( MotorInterface motor){
    fMotor = motor;
  };
  
  public int getRealSpeed(){ return fRealSpeed;};
  
  /**
   * update. Must frequent to keep high precision
   * @throws ConnectionLostException 
   */
  public void update() throws ConnectionLostException{
    
    if( fMotor == null ){
      return;
    }
     
    //call motor controller update
    fMotor.update();
    
    // get encoder value
    // take care that encoder are in opposite side. Conventionally let's inverse right encoder value.
    int currEncoderValue = fMotor.getEncoderValue();
    
    fRealSpeed = currEncoderValue - fLastCurrEncoderValue;
    fLastCurrEncoderValue = currEncoderValue;

    // Update the position
    fPosition = updatePosition( currEncoderValue );
    // Log.d("LocoSingle", "position:" + Double.toString( fPosition) );
    
    
    // get the difference between current position and target
    double target = getRelativeTargetPosition( fTarget );
    
    // if the target already reached return
    //if( fTargetReached )
    //  return;
    
    // Determine if the target is reach
    if( Math.abs( target ) < fPrecision )
    {  
      fTargetReached = true;
      Log.i("LocoSingle", "target reached");
      fMotor.setSpeed( 0.0f );
      return;
    }else
    {  
      fTargetReached = false;
    }
    
    // if the stop command has been called
    // overwrite target distance
    if( fRegularFreeze )
    {
      target = 0.f;
      fRegularFreeze = false;
    }

    // apply the acceleration limitation
    if( fRampEnable ) target = xAccLimit( target );
    
    // send new command to the pid
    fPid.setCurrValue( target, 1 );

    // compute pid 
    double newSpeed = fPid.update( );

    // Mixes angle command and straight command
    double speed = newSpeed;

    // energencyStop method has been called
    if( fEmergencyFreeze){
      Log.i("LocoSingle", "Emergency freeze has been called. LocomotionSingle stop!");
      fMotor.setSpeed(0.);
    }else
    {
      // callback motor controller to send the speed command
      //ODO_DEBUG_LOCOSINGLE( newSpeed  << "\t" << currEncoderValue << "\t" << target );
      fMotor.setSpeed( fMaxSpeed * speed );
    }
  }

  /**
   * set current Target.
   * @param target new Target to be reached. <BR>
   * A specific case is done if target.x and target.y are set to 0, 
   * then we consider target as a angle target, in this case X and Y coordinates are ignored.
   * d is systematically ignored
   */
  public void setTarget( double target ){
    fTarget = target;
    fTargetReached = false;
    Log.i( "LocoSingle", "new target is " + Double.toString(fTarget) ); 
  }

  /**
   * @return the current robot target
   */
  public double getTarget(){ 
    return fTarget; 
  };
  
  /**
   * @return true if target position is under the defined precision
   */
  public boolean isTargetReached(){ 
    return fTargetReached;
  };
  
  
  /*======================
   * accessors
  ====================== */
  /**
   * accessors to set the PID parameters of the distance control
   */ 
  /** @param p set coefficients of the pid regulator */
  public void setPID( double p, double i, double d) 
  { 
    fPid.P = p; 
    fPid.I = i; 
    fPid.D = d; 
  };

  /** @return the current coefficients of the distance pid regulator */
  public double[] getPID()        { return new double[]{fPid.P, fPid.I, fPid.D}; };
  
  /** @param pDistance Desire angle precision of the angle regulator */
  public void setPrecision( double precision ){ fPrecision = precision; };
  
  /** @return the desire distance precision of the angle regulator */
  public double getPrecision( ){ return fPrecision; };
  
  /** @param maxSpeed Maximum motor speed */
  public void setMaxSpeed( double maxSpeed ) { fMaxSpeed = maxSpeed; };
  
  /** @return Current maximum motor speed */
  public double getMaxSpeed( ) { return fMaxSpeed; };

  /** @param accLimit Distance target acceleration limit */
  public void setAccLimit(double accLimit){ fAccLimit = accLimit; };
  
  /** @return the current distance target acceleration limit */
  public double getAccLimit(){ return fAccLimit;};

  
  /**
   * Stop with respect of the limit acceleration criteria.
   * These methods needs to be called befaire each update !
   */
  public void freeze(){ fRegularFreeze = true; };
    
  /**
   * Stop the robot as soon as possible without respect 
   * of the acceleration limit.
   */
  public void emergencyFreeze(){ fEmergencyFreeze = true; };
  
  /**
   * Release the robot from emergency freeze state.
   */ 
  public void emergencyUnfreeze(){ fEmergencyFreeze = false; };
  
  /**
   * Disable distance regulator acceleration ramp
   */
  public void setRampEnable( boolean onOff ) { fRampEnable = onOff; };
  
    
  /**
   * Apply a limit acceleration on the Distance pid regulator input. This limit is define by the user.
   */
  private double xAccLimit( double in ){
    double tmp = in;
    if( in < fOldDelta - fAccLimit)
      tmp = fOldDelta - fAccLimit;
    if( in > fOldDelta + fAccLimit)
      tmp = fOldDelta + fAccLimit;
    fOldDelta = tmp;
    return tmp;
  }

  /** pid parameters of the distance regulator */
  private PID fPid;
  
  /**
   * current position
   */
  private double fPosition;
  
  /**
   * current target
   */
  private double fTarget;
  
  /** Pointer to the left motor controller */
  private MotorInterface fMotor;
  
  /** if the target is reached */
  private boolean fTargetReached;
  
  /** precision values (angle, distance) */
  private volatile double fPrecision;
  
  /** max speed */
  private volatile double fMaxSpeed;
  
  /** backup of the previous value. 
   * Use to apply ramp on the pid input data*/
  private double fOldDelta;
  
  /** acceleration limits */
  private volatile double fAccLimit;
  
  /** stop signals */
  private boolean fRegularFreeze;
  private boolean fEmergencyFreeze;
  
  /** disable ramp signal */
  private boolean fRampEnable;
  
  private int fLastCurrEncoderValue;
  
  /** Speed measurement */
  private int fRealSpeed;
  
}
